#include "../src/dyadicp.cc"

#include <cassert>
#include <iostream>

namespace
{
  struct PFuncTestData
    {
      float s;
      float adotb;
      float anormplusnorm;
      float wdotx;
      float xnorm;
      float kappa;
      float p;
    };

  std::ostream&
  operator<< (std::ostream& o,
              const PFuncTestData& t)
    {
      return o << "{ " 
               << t.s << ","
               << t.adotb << ","
               << t.anormplusnorm << ","
               << t.wdotx << ","
               << t.xnorm << ","
               << t.p 
               << "}";
    }
}

#include "pfunctestdata.cc"

namespace
{
  std::ostream&
  operator<< (std::ostream& o,
              std::pair<bool, float>& p)
    {
      return o << "{ " << (p.first ? "true" : "false") 
               << ", " << p.second
               << " }";
    }
}

namespace
{
  bool
  fuzzy_eq (float a,
            float b)
    {
      return fabs (a - b) <= 1e-5 * (1.0 + fabs (a) + fabs (b));
    }

  void
  test_pfunc ()
    {
      unsigned int precision_fail = 0;
      unsigned int computation_fail = 0;

      for (unsigned int n = 0;
           n < sizeof (test_data) / sizeof (test_data[0]);
           ++n)
        {
          std::pair<bool, float> p = 
            flassol::dyadic::p (test_data[n].s,
                                test_data[n].adotb,
                                test_data[n].anormplusnorm,
                                test_data[n].wdotx,
                                test_data[n].xnorm,
                                test_data[n].kappa);

          assert (fuzzy_eq (test_data[n].p, p.second)
                  || p.first
                  || (std::cerr << "test_data = " << test_data[n]
                                << " p = " << p 
                                << std::endl,
                      0));

          if (p.first) { ++precision_fail; }
          if (! fuzzy_eq (test_data[n].p, p.second)) { ++computation_fail; }
        }

      std::cerr << __FUNCTION__ 
                << " info: " << precision_fail 
                << " out of " << sizeof (test_data) / sizeof (test_data[0])
                << " catastrophic cancellations estimated"
                << std::endl
                << __FUNCTION__
                << " info: " << computation_fail
                << " actual incorrect computations"
                << std::endl;
    }

  bool
  test_pinvfunc_once ()
    {
      float a = (drand48 () < 0.25) ? 0 : 2.0 * drand48 () - 1.0;
      float b = (drand48 () < 0.25) ? 0 : 2.0 * drand48 () - 1.0;
      float x = 0.5f + drand48 ();

      float w = drand48 () - 0.5;
      float adotb = a * b;
      float anormplusnorm = a * a + b * b;
      float wdotx = w * x;
      float xnorm = x * x;
      float kappa = drand48 ();
      float p = 2.0 * drand48 () - 1.0;

      std::pair<bool, float> s = 
        flassol::dyadic::pinv (p, adotb, anormplusnorm, wdotx, xnorm, kappa);

      std::pair<bool, float> ps = 
        flassol::dyadic::p (s.second, adotb, anormplusnorm, wdotx, xnorm, kappa);

      assert (fuzzy_eq (ps.second, p) || s.first || 
              (std::cerr << " p = " << p
                         << " s = " << s 
                         << " ps = " << ps
                         << " a = " << a
                         << " b = " << b
                         << " x = " << x
                         << " fabs (" << ps.second << " - " 
                                      << p << ") = " 
                                      << fabs (ps.second - p)
                         << " 1.0 + fabs (" << ps.second << ") + "
                                   "fabs (" << p << ") = "
                                            << 1.0 + fabs (ps.second) 
                                                   + fabs (p)
                         << std::endl,
               0));

      return s.first;
    }

  void
  test_pinvfunc ()
    {
      unsigned int precision_fail = 0;
      unsigned int count = 1000000;

      for (unsigned int n = 0; n < count; ++n)
        {
          if (test_pinvfunc_once ())
            {
              ++precision_fail;
            }
        }

      std::cerr << __FUNCTION__ 
                << " info: " << precision_fail 
                << " out of " << count << " failures to invert detected"
                << std::endl;
    }
}

int 
main ()
{
  srand48 (69);

  test_pfunc ();
  test_pinvfunc ();

  return 0;
}
